/* ********************************************************************** */
/*                           Easy IMU Pilot                               */
/*                                                                        */
/* Code based on ArduIMU DCM code from Diydrones.com                      */
/* and on JJ ArduIMU Quadcopter (from Jose Julio) v1.20 (22/02/2010)      */
/* Date : 13/06/2010                                                      */
/* Author : Loris Rion                                                    */
/* Version : 1.74                                                         */
/* ********************************************************************** */


//Renvoyer Omega_I et Omega_P
//Diminuer les gains DCM ?
//finir performance reporting
//use Omega or Omega_Vector ????


#include <Wire.h>   // For magnetometer readings
#include <ServoDecode.h>  // PPM decoder library from mem http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1228137503

#include "config_imu.h"
#include "radio_esky.h" // Radio settings
//#include "airframe_flyingwing.h"
//#include "airframe_mavion.h"
//#include "airframe_mavion2.h"
#include "airframe_miniquad.h"

#include <ServoTimer2Fast.h> 
#if OUTPUT_SERVOTIMER2 == 1  // Use SerialTimer2 library
#define SERVOLIB <ServoTimer2Fast.h>  // Servo library using timer 2 from mem http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1230479947
#include SERVOLIB

ServoTimer2 servo0;
ServoTimer2 servo1; 
ServoTimer2 servo2;
ServoTimer2 servo3;
#endif


#if USE_TX_BUFFER == 1  // Use Serial TX Buffer library
#define SERIALLIB <HardwareSerialBuf.h>
#include SERIALLIB
#define SerialPort SerialBuf
#else                   // Use Arduino Serial library
#define SerialPort Serial
#endif


// ADC variables
volatile byte MuxSel=0;
volatile byte analog_reference = EXTERNAL;
volatile unsigned int analog_buffer[8]; //buffer for ADC reading
volatile byte analog_count[8];
volatile unsigned int AN_raw[8]; //store the ADC raw data in 12 bits
unsigned int AN_OFFSET[6]; //Array that stores the Offset
float AN[6]; //array that store the 6 ADC filtered data

float Accel_Vector[3];  //Store the acceleration in a vector (m/s2)
float Gyro_Vector[3];   //Store the gyros turn rate in a vector (rad/s)
float Omega_Vector[3];  //Corrected Gyro_Vector data (rad/s)
float Omega_P[3];       //Omega Proportional correction (rad/s)
float Omega_I[3]= {
  0,0,0};               //Omega Integrator correction (rad/s)
float Omega[3];         //Omega is the raw gyro reading plus Omega_I, so it´s bias corrected (rad/s)
float errorRollPitch[3];
float errorYaw[3];

// GPS DATA
float ground_speed = 0;
float ground_course = 0;//3D speed (estimated here because we don't have a GPS) in m/s
float speed_3d = 0;

//Euler angles in radian
float roll;
float pitch;
float yaw;

//Magnetometer variables
int magnetom_x;
int magnetom_y;
int magnetom_z;
float MAG_Heading;


float DCM_Matrix[3][3]= {
  {
    1,0,0                      }
  ,{
    0,1,0                      }
  ,{
    0,0,1                      }
}; 

unsigned int ch[NB_CHANNELS + 1];    // Channel values from rc (us)

struct TPID { //Structure de correcteur PID
  //Gains (us/°)
  float Pgain; // Proportionnal gain
  float Igain; // Integrator gain
  float Dgain; // Derivative gain

    float Pterm; // Proportionnal term (in us)
  float Dterm; // Derivative term (in us)
  float Iterm; // Integrator term (in us)

  int control; // Command (result of commmand law) in delta us
};

struct TCommande { //Structure de commande rx (in ° or from 0 to 1 for throttle)
  float current; // in °
  float old; // in °
  float diff; // in °/s
};

//****** Attitude control variables
TPID PIDroll;
TPID PIDpitch;
TPID PIDyaw;
float kpor = K_PITCHofROLL;

//commands (from rc)
TCommande command_roll; // in ° (+- MAX_ROLL)
TCommande command_pitch; // in ° (+- MAX_PITCH)
TCommande command_yaw; // in ° (+- 180°)
TCommande command_throttle; //0 to 1

//commands (results of commmand laws) in delta us
int control_roll;
int control_pitch;
int control_yaw;
int control_throttle;

// Autopilot mode
byte AP_mode = FLY_BY_WIRE;  

// timer variables
unsigned int counter = 0;
float G_Dt;    // Integration time for the gyros (DCM algorithm)
long timer = 0; //general purpose timer 
long timer_old = 0;


//performance variables
#if PERFORMANCE_REPORTING == 1  
float G_Dt_max = 0;
unsigned int gyro_sat_count = 0;
float t_dcm = 0;
float t_read = 0;
unsigned int nb_dcm;
float t_link = 0;
unsigned long timer_tmp;
unsigned long timer_tmp_old;
#endif


float G_Dt_max = 0;

/* ************************************************************ */
void loop(){

  if((DIYmillis()-timer) >= (1000/AHRS_LOOP)) //AHRS LOOP
  {
    timer_old = timer;
    timer = DIYmillis();
    G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)

//#if PERFORMANCE_REPORTING == 1  
    if (G_Dt > G_Dt_max) G_Dt_max = G_Dt; // keep max g_dt time in memory
//#endif

    counter++;
#if USE_MAGNETOMETER == 1    //if you have a magnetometer
    if (counter % (AHRS_LOOP/MAGNETOMETER_LOOP) == 0) //MAGNETOMETER LOOP
    {
      Read_Compass();    // Read I2C magnetometer
      Compass_Heading(); // Calculate magnetic heading  
    }
#endif

    Read_adc_raw(); //Read ADC (accelero & gyros)

#if PERFORMANCE_REPORTING == 1  
    timer_tmp_old = micros();
#endif
    DCM();  // IMU DCM Algorithm
#if PERFORMANCE_REPORTING == 1  
    timer_tmp = micros();
    t_dcm += timer_tmp - timer_tmp_old;
    nb_dcm++;
#endif


#if PERFORMANCE_REPORTING == 1  
    // Check gyros saturations
    if((abs(Gyro_Vector[0])>=radians(300))||(abs(Gyro_Vector[1])>=radians(300))||(abs(Gyro_Vector[2])>=radians(300)))
      gyro_sat_count++;
#endif

    if (counter % (AHRS_LOOP/RADIO_LOOP) == 0) //CONTROL LOOP
    {
      readRadio(); //read radio commands
    }

    if (counter % (AHRS_LOOP/CONTROL_LOOP) == 0) //CONTROL LOOP
    {
#if PERFORMANCE_REPORTING == 1  
      timer_tmp_old = micros();
#endif
      Attitude_control(); //PID algorithm
      Mixing(); //write servos commands
#if PERFORMANCE_REPORTING == 1  
      timer_tmp = micros();
      t_read += timer_tmp - timer_tmp_old;
#endif
    }

    if (counter % (AHRS_LOOP/TELEMETRY_LOOP) == 0) //TELEMETRY LOOP
    {
      digitalWrite(5, LOW); // Light off Red led
#if PERFORMANCE_REPORTING == 1  
      timer_tmp_old = micros();
#endif
      check_uplink();//check if uplink data available
      printdata(); //telemetry
#if PERFORMANCE_REPORTING == 1  
      timer_tmp = micros();
      t_link += timer_tmp - timer_tmp_old;
#endif
    }
  }
}


